/*******************************************************************************
*
* (c) Copyright 2014 Freescale Semiconductor
*
****************************************************************************//*!
*
* @file     Accelerometer.c
*
* @author   Salvador Zendejas
*
* @version  0.0.1
*
* @date      Nov 15, 2014
*
* @brief    Accelerometer driver for MQX RTOS
*
*******************************************************************************/
#include "Accelerometer.h"

MQX_FILE_PTR fd_accel;

/*FUNC*-----------------------------------------------------
* 
* FUNC Name    : _io_accelerometer_install
* Comments     :
*    Driver for an Accelerometer Set up the driver
*    and gets the file descriptor
*
*END*-----------------------------------------------------*/
_mqx_int _io_accelerometer_install(void)
{
	printf("Accelerometer Driver install\n");
	DEBUG_LOG("\n Starting I2C application to read data from an Accelerometer device\n");
	DEBUG_LOG("\n Opening I2C...");

	fd_accel = fopen(I2C_DEVICE_INTERRUPT, NULL);		// Open I2C file descriptor and returns and object
	
	if (NULL != fd_accel){					// If the descriptor != NULL, everything is going ok
		DEBUG_LOG(TABS+"OK\n");
	}else{
		DEBUG_LOG(TABS+"Error loading file\n");
		return ACCEL_ERROR;
	}
	return ACCEL_OK;
}

/*FUNC*-----------------------------------------------------
* 
* FUNC Name    : _io_accelerometer_open
* Comments     :
*    Driver for an Accelerometer device with I2C communitacion
*
*END*-----------------------------------------------------*/
_mqx_int _io_accelerometer_open(void)
{
	_mqx_int     i2c_param;
	SIM_SCGC1 |= (SET<<SIM_SCGC1_I2C2_SHIFT);

	printf("Accelerometer Driver Open...\n");
	
	DEBUG_LOG("Board in Master mode for I2C communication...");
	if(I2C_OK == ioctl(fd_accel, IO_IOCTL_I2C_SET_MASTER_MODE, NULL)){	// Sets uProc in master mode.
		DEBUG_LOG(TABS+"OK\n");
	}else{
		DEBUG_LOG(TABS+"ERROR\n");
		return ACCEL_ERROR;
	}
	
	i2c_param = SLAVE_ADDRESS;				//Accelerometer slave address (by HW)
	printf(" Set I2C bus address to 0x%02x...",i2c_param);								
	if(I2C_OK == ioctl(fd_accel, IO_IOCTL_I2C_SET_DESTINATION_ADDRESS, &i2c_param)){			// Set message destination address, how ioctl works is MQX_OK(file_descriptor, command, pointer_io_ctl_param)
		printf(TABS+"OK\n");		
	}else{
		printf(TABS+"ERROR\n");
		return ACCEL_ERROR;
	}

	addr_reg[0] = CTRL_REG1;			// Control register 1 address
	addr_reg[1] = WHO_AM_I;		// WHO_AM_I register address
	DEBUG_LOG(" To place device into Standby...");
	result = fwrite(addr_reg,BYTE,2,fd_accel);			// Take it as faith dogma (but we are assuming that the message is send here)
	if(2 == result){
		DEBUG_LOG(TABS+"OK\n");
	}else{
		DEBUG_LOG(TABS+"ERROR\n");
	}
	
	DEBUG_LOG(" Send repeated start...");
	if(I2C_OK == ioctl(fd_accel, IO_IOCTL_I2C_REPEATED_START, NULL)){			// Send repeated start because of protocol (Initialize I2C again)
		DEBUG_LOG(TABS+"OK\n");
	}else{
		DEBUG_LOG(TABS+"ERROR\n");
	}

	/*Ponerse en mode de recepcion*/
	i2c_param = 6;
	DEBUG_LOG("Request %d data...",i2c_param);
	if(I2C_OK == ioctl(fd_accel, IO_IOCTL_I2C_SET_RX_REQUEST, &i2c_param)){		// Set uProc in RX mode and wait for retrieve a data 
		DEBUG_LOG("OK\n");
	}else{
		DEBUG_LOG("ERROR\n");
	}

	DEBUG_LOG("Reading bytes...");												// Read data
	result = fread(acc_data,1,i2c_param,fd_accel);
	if(i2c_param == result){
		DEBUG_LOG("OK\n");
	}else{
		DEBUG_LOG("ERROR\n");
	}			
	DEBUG_LOG("Stop bit...");
	if(I2C_OK == ioctl(fd_accel, IO_IOCTL_I2C_STOP, NULL)){					// Send stop bit
		DEBUG_LOG("OK\n");
	}else{
		DEBUG_LOG("ERROR\n");
	}			
	DEBUG_LOG("Setting slave\n");
	if(I2C_OK == ioctl(fd_accel, IO_IOCTL_I2C_SET_SLAVE_MODE, NULL)){			// Set again in slave mode
		DEBUG_LOG("OK\n");
	}else{
		DEBUG_LOG("ERROR\n");
	}

	return ACCEL_OK;
}

/*FUNC*-----------------------------------------------------
* 
* Func Name    : _io_accelerometer_ioctl
* Comments     :
*    implementsget and set commands, provides different
*    configuration settings for the driver
*
*END*-----------------------------------------------------*/
_mqx_int _io_accelerometer_ioctl(_mqx_uint command, void *param){
	switch (command) {
		case IO_IOCTL_ACCELEROMETER_SET_G_MODE:
			io_accelerometer_write(&param);
			break;
		case IO_IOCTL_ACCELEROMETER_GET_G_MODE:
			io_accelerometer_read();
			break;
		default:
			break;
	}
}
/*FUNC*-----------------------------------------------------
* 
* Task Name    : Accelerometer_Install
* Comments     :
*    Driver for an Accelerometer device with I2C communitacion
*
*END*-----------------------------------------------------*/

void rx_data(void)
{
	_mqx_int     i2c_param;
	
	uint8_t acc_addresses[2];	// Keeps address 
	uint8_t acc_data[16];		// Data buffer
	uint16_t dataX;
	uint16_t dataY;
	uint16_t dataZ;
	_mqx_int result;
	
	
	printf("\nBoard in Master mode for I2C communication...");
	if(I2C_OK == ioctl(fd_accel, IO_IOCTL_I2C_SET_MASTER_MODE, NULL)){	// Sets uProc in master mode. Seting master mode using IO_IOCTL_I2C_SET_MASTER_MODE and the I2C descriptor.
																		/* ioctl() Do a poll to set I2C commands */
		printf("OK\n");
	}else{
		printf("ERROR\n");
	}	
	
	i2c_param = SLAVE_ADDRESS;				//accelerometer slave address
	printf(" Set I2C bus address to 0x%02x...",i2c_param);								
	if(I2C_OK == ioctl(fd_accel, IO_IOCTL_I2C_SET_DESTINATION_ADDRESS, &i2c_param)){			// Set message destination address, how ioctl works is MQX_OK(file_descriptor, command, pointer_io_ctl_param)
		printf("OK\n");		
	}else{
		printf("ERROR\n");
	}
	
	acc_addresses[0]=CTRL_REG1;			// Control register 1 address
	acc_addresses[1]=WHO_AM_I;		// WHO_AM_I register address
	printf("Writing addresses...");
	result = fwrite(acc_addresses,1,2,fd_accel);			// Take it as faith dogma (but we are assuming that the message is send here)
	
	if(2 == result){
		printf("OK\n");
	}else{
		printf("ERROR\n");
	}
#if (CONTINOUS_DATA)	
	printf("Generate stop bit ... ");					
	if(I2C_OK == ioctl(fd_accel, IO_IOCTL_I2C_STOP, NULL)){		// Send stop bit and ending master mode -> Preparing to change to slave mode
		printf("OK\n");
	}else{
		printf("ERROR\n");
	}
	
	/*Slave mode*/
	printf("Receiving ACK...");
	if(I2C_OK == ioctl(fd_accel, IO_IOCTL_I2C_SET_SLAVE_MODE, NULL)){			// Set uProc to slave mode -> Will be waitng for accelerometer data
		printf("OK\n");
	}else{
		printf("ERROR\n");
	}
	
	printf("Ending Transaction...");
	if(I2C_OK == ioctl(fd_accel, IO_IOCTL_I2C_SET_MASTER_MODE, NULL)){
		printf("OK\n");
	}else{
		printf("ERROR\n");
	}
#else
	
#endif
	
		acc_addresses[0] = X_MSB_REG;			//Since first reg that I want to read
		DEBUG_LOG("Writing Init Reg to read...");
		result = fwrite(acc_addresses,1,1,fd_accel);	// We send it with the hope to get 1 byte back
		if(1 == result){							// Check if we read some shiz
			DEBUG_LOG("OK\n");
		}else{
			DEBUG_LOG("ERROR\n");
		}			
		DEBUG_LOG("Send repeated start..");
		if(I2C_OK == ioctl(fd_accel, IO_IOCTL_I2C_REPEATED_START, NULL)){			// Send repeated start because of protocol (Initialize I2C again)
			DEBUG_LOG("OK\n");
		}else{
			DEBUG_LOG("ERROR\n");
		}
		
		/*Ponerse en mode de recepcion*/
		i2c_param = 6;
		DEBUG_LOG("Request %d data...",i2c_param);
		if(I2C_OK == ioctl(fd_accel, IO_IOCTL_I2C_SET_RX_REQUEST, &i2c_param)){		// Set uProc in RX mode and wait for retrieve a data 
			DEBUG_LOG("OK\n");
		}else{
			DEBUG_LOG("ERROR\n");
		}
		
		DEBUG_LOG("Reading bytes...");												// Read data
		result = fread(acc_data,1,i2c_param,fd_accel);
		if(i2c_param == result){
			DEBUG_LOG("OK\n");
		}else{
			DEBUG_LOG("ERROR\n");
		}			
		DEBUG_LOG("Stop bit...");
		if(I2C_OK == ioctl(fd_accel, IO_IOCTL_I2C_STOP, NULL)){					// Send stop bit
			DEBUG_LOG("OK\n");
		}else{
			DEBUG_LOG("ERROR\n");
		}			
		DEBUG_LOG("Setting slave\n");
		if(I2C_OK == ioctl(fd_accel, IO_IOCTL_I2C_SET_SLAVE_MODE, NULL)){			// Set again in slave mode
			DEBUG_LOG("OK\n");
		}else{
			DEBUG_LOG("ERROR\n");
		}	
		dataX = (acc_data[0] << 8 | acc_data[1]) >> 2;
		dataY = (acc_data[2] << 8 | acc_data[3]) >> 2;
		dataZ = (acc_data[4] << 8 | acc_data[5]) >> 2;
		

		printf("X = %x, Y = %x, Z = %x            \r",dataX,dataY,dataZ);


	_mqx_exit(0);
}

/* EOF */
